#include "Includes.h"



#pragma region //===变量===
extern osSemaphoreId I2C1_DoneSemHandle;
extern osSemaphoreId SPI2_DoneSemHandle;
extern osSemaphoreId RxCodEndSemHandle;
extern osSemaphoreId SPI1_DoneSemHandle;
//SPI1由TLA2518和MT29F1G共用,故申请互斥信号量
extern osMutexId SPI1_AccessMutexHandle;
extern osMessageQId PC_TxMsgQueueHandle;

//16路编码器角度值[度],前一角度值, 角速度[度/s]
int Angle[16], angle0[16];
float Speed[16];
uint32_t smpl_time[16];

//12个电流采集值[mA]
float Current[12];
//电机电源电压[V], ADC_AUX电源电压[V]
uint32_t Vmotor, Vadc_aux;

//12个电流标定值和校准数据
//I*Rc*G=data*3.3/2^12 => I=data*(3.3/(Rc*G))/2^12, G=80预估:CalIc=20625mA, CalIc_data=65535
float CalIc[12], CalIc_data[12];
//电机电源电压标定值和校准数据, 预估:CalVm=33000mV, CalVm_data=65535--16位
uint16_t CalVm = 33000, CalVm_data = 65535;
//ADC_AUX电源电压标定值和校准数据
uint16_t CalVa = 33000, CalVa_data = 65535;

StatusInfo_t status_info = { .sync = 0xFFFF };

int WorkMode;	//控制电机工作模式,见MORK_MODE定义
uint8_t NowMotor;	//当前操作对象--电机序号0~11

int pos_am;
int pos_tc;
int spd_am;
int spd_tc;
int cur_am;
int cur_tc;

extern eeprom_var_t eeprom_var;
#pragma endregion

#pragma region //===字段===
#define SAMPLE_T	40	//编码器数据采样周期ms

//硬件连接配置
const hd_link_t hd_link = {
	//芯片编号:0(U15),1(U16),2(U17)//组:0=1,2;1=3,4;2=5,6;3=7,8; //PWM编号:0=PWM(0+cs*3); 1=PWM2; 2=PWM3;//编码器编号:0~15;//电流通道索引号:0~15;
	.hd_cfg[0] = { 0, 0, 0, 0, 0 },
	.hd_cfg[1] = { 0, 1, 1, 1, 1 },
	.hd_cfg[2] = { 0, 2, 2, 2, 2 },
	.hd_cfg[3] = { 0, 3, 1, 3, 3 },
	
	.hd_cfg[4] = { 1, 0, 0, 4, 4 },
	.hd_cfg[5] = { 1, 1, 1, 5, 1 },
	.hd_cfg[6] = { 1, 2, 2, 6, 2 },
	.hd_cfg[7] = { 1, 3, 1, 7, 3 },
	
	.hd_cfg[8] = { 2, 0, 0, 4, 4 },
	.hd_cfg[9] = { 2, 1, 1, 5, 1 },
	.hd_cfg[10] = { 2, 2, 2, 6, 2 },
	.hd_cfg[11] = { 2, 3, 1, 7, 3 },
};
pos_spd_cur_err_t errs[12];
uint8_t last_pwm_dir[12]; //上一次pwm方向:0--正转,1--反转,其它--没初始化
uint32_t pos_t, spd_t, cur_t;
#pragma endregion#pragma region //==============私有函数==============
/*
********************************************************************************
*描    述 : 分别采集16路编码器数据。结果在Angle[]中.计算转速在Speed[]中
*返    回 : 0-失败, 1-成功
********************************************************************************/
	int rd;
int ScanCoder(int index/*编码器索引号*/)
{
	switch (index)
	{
	case 0:
		HAL_GPIO_WritePin(CODER_ADDR0_GPIO_Port, CODER_ADDR0_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR1_GPIO_Port, CODER_ADDR1_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR2_GPIO_Port, CODER_ADDR2_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR3_GPIO_Port, CODER_ADDR3_Pin, GPIO_PIN_RESET);
		break;
	case 1:
		HAL_GPIO_WritePin(CODER_ADDR0_GPIO_Port, CODER_ADDR0_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR1_GPIO_Port, CODER_ADDR1_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR2_GPIO_Port, CODER_ADDR2_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR3_GPIO_Port, CODER_ADDR3_Pin, GPIO_PIN_RESET);
		break;
	case 2:
		HAL_GPIO_WritePin(CODER_ADDR0_GPIO_Port, CODER_ADDR0_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR1_GPIO_Port, CODER_ADDR1_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR2_GPIO_Port, CODER_ADDR2_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR3_GPIO_Port, CODER_ADDR3_Pin, GPIO_PIN_RESET);
		break;
	case 3:
		HAL_GPIO_WritePin(CODER_ADDR0_GPIO_Port, CODER_ADDR0_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR1_GPIO_Port, CODER_ADDR1_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR2_GPIO_Port, CODER_ADDR2_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR3_GPIO_Port, CODER_ADDR3_Pin, GPIO_PIN_RESET);
		break;
	case 4:
		HAL_GPIO_WritePin(CODER_ADDR0_GPIO_Port, CODER_ADDR0_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR1_GPIO_Port, CODER_ADDR1_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR2_GPIO_Port, CODER_ADDR2_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR3_GPIO_Port, CODER_ADDR3_Pin, GPIO_PIN_RESET);
		break;
	case 5:
		HAL_GPIO_WritePin(CODER_ADDR0_GPIO_Port, CODER_ADDR0_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR1_GPIO_Port, CODER_ADDR1_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR2_GPIO_Port, CODER_ADDR2_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR3_GPIO_Port, CODER_ADDR3_Pin, GPIO_PIN_RESET);
		break;
	case 6:
		HAL_GPIO_WritePin(CODER_ADDR0_GPIO_Port, CODER_ADDR0_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR1_GPIO_Port, CODER_ADDR1_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR2_GPIO_Port, CODER_ADDR2_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR3_GPIO_Port, CODER_ADDR3_Pin, GPIO_PIN_RESET);
		break;
	case 7:
		HAL_GPIO_WritePin(CODER_ADDR0_GPIO_Port, CODER_ADDR0_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR1_GPIO_Port, CODER_ADDR1_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR2_GPIO_Port, CODER_ADDR2_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR3_GPIO_Port, CODER_ADDR3_Pin, GPIO_PIN_RESET);
		break;
	case 8:
		HAL_GPIO_WritePin(CODER_ADDR0_GPIO_Port, CODER_ADDR0_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR1_GPIO_Port, CODER_ADDR1_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR2_GPIO_Port, CODER_ADDR2_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR3_GPIO_Port, CODER_ADDR3_Pin, GPIO_PIN_SET);
		break;
	case 9:
		HAL_GPIO_WritePin(CODER_ADDR0_GPIO_Port, CODER_ADDR0_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR1_GPIO_Port, CODER_ADDR1_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR2_GPIO_Port, CODER_ADDR2_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR3_GPIO_Port, CODER_ADDR3_Pin, GPIO_PIN_SET);
		break;
	case 10:
		HAL_GPIO_WritePin(CODER_ADDR0_GPIO_Port, CODER_ADDR0_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR1_GPIO_Port, CODER_ADDR1_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR2_GPIO_Port, CODER_ADDR2_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR3_GPIO_Port, CODER_ADDR3_Pin, GPIO_PIN_SET);
		break;
	case 11:
		HAL_GPIO_WritePin(CODER_ADDR0_GPIO_Port, CODER_ADDR0_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR1_GPIO_Port, CODER_ADDR1_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR2_GPIO_Port, CODER_ADDR2_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR3_GPIO_Port, CODER_ADDR3_Pin, GPIO_PIN_SET);
		break;
	case 12:
		HAL_GPIO_WritePin(CODER_ADDR0_GPIO_Port, CODER_ADDR0_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR1_GPIO_Port, CODER_ADDR1_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR2_GPIO_Port, CODER_ADDR2_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR3_GPIO_Port, CODER_ADDR3_Pin, GPIO_PIN_SET);
		break;
	case 13:
		HAL_GPIO_WritePin(CODER_ADDR0_GPIO_Port, CODER_ADDR0_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR1_GPIO_Port, CODER_ADDR1_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR2_GPIO_Port, CODER_ADDR2_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR3_GPIO_Port, CODER_ADDR3_Pin, GPIO_PIN_SET);
		break;
	case 14:
		HAL_GPIO_WritePin(CODER_ADDR0_GPIO_Port, CODER_ADDR0_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(CODER_ADDR1_GPIO_Port, CODER_ADDR1_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR2_GPIO_Port, CODER_ADDR2_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR3_GPIO_Port, CODER_ADDR3_Pin, GPIO_PIN_SET);
		break;
	case 15:
		HAL_GPIO_WritePin(CODER_ADDR0_GPIO_Port, CODER_ADDR0_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR1_GPIO_Port, CODER_ADDR1_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR2_GPIO_Port, CODER_ADDR2_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(CODER_ADDR3_GPIO_Port, CODER_ADDR3_Pin, GPIO_PIN_SET);
		break;
	default:
		return 0;
	}
	
	//data => angle
	MagStatus_t ms;
	rd = MT6701_GetRawData(&ms);
//	if (ms.byte != 0 && ms.byte != 4)
//	{
//		return 0;
//	}
	Angle[index] = rd & 0x1FFF; // * (360.0f / 8192.0f);
	uint32_t dt = osKernelSysTick() - smpl_time[index];
	smpl_time[index] = dt + smpl_time[index];
	
	//计算速度,当Angle0<0(初始化)只初值Angle0不计算速度
	if (angle0[index] >= 0)
	{
		int del = Angle[index] - angle0[index];
		//求最小夹角
		if (del > 4096) //180.F)
		{
			del -= 8192; //360.F;
		}
		else if(del < -4096) //180.F)
		{
			del += 8192; //360.F;
		}
		Speed[index] = Speed[index] * 0.7f + del * (1000.f*0.3f) / dt; //angle/ms*1000=>angle/s
	}
	angle0[index] = Angle[index];
	return 1;
}
/*
********************************************************************************
*描    述 : 分别采集16路ADC转换数据。
*返    回 : 0-失败, 1-成功
********************************************************************************/
static int ScanADC(int index/*通道索引号*/)
{
	uint32_t data;
	if (index >= 16)
	{
		return 0;
	}
	if (index < 8)
	{
		data = TLA2518_GetADC(index);
		if (index == 6)
		{
			HAL_GPIO_WritePin(ADC_CHAN_SEL_GPIO_Port, ADC_CHAN_SEL_Pin, GPIO_PIN_SET); //选中NO通道电流检测
			Vmotor = data * CalVm / CalVm_data;
			return 1;
		}
		if (index == 7)
		{
			Vadc_aux = data * CalVa / CalVa_data;
			return 1;
		}
	}
	else
	{
		data = TLA2518_GetADC(index - 8); //adc循环
		if (index == 14)
		{
			HAL_GPIO_WritePin(ADC_CHAN_SEL_GPIO_Port, ADC_CHAN_SEL_Pin, GPIO_PIN_RESET); //选中NC通道电流检测
			Vmotor = data * CalVm / CalVm_data;
			return 1;
		}
		if (index == 15)
		{
			Vadc_aux = data * CalVa / CalVa_data;
			return 1;
		}
		index -= 2; //每8路AIN最后2路不是电流
	}
	
	Current[index] = data * CalIc[index] / CalIc_data[index]; //I*Rc*G=data*3.3/2^12 => I=data*(3.3/(Rc*G))/2^12	
	return 1;
}
/*
********************************************************************************
*描    述 : 分别采集16路ADC转换数据。
*返    回 : 0-失败, 1-成功
********************************************************************************/
int SC_PackAngleSpeedFr(uint8_t *buf)
{
	return 0;
}

/*
********************************************************************************
*描    述 : 角度pid控制。
*返    回 : 角度控制值
********************************************************************************/
static float angle_pid_ctrl(int new_err/*新误差值*/, error_t *err/*旧误差*/, PID_t *pid/*控制参数*/)
{
	//求最小夹角
	if (new_err > 4096) //180°
	{
		new_err -= 8192; //360.F;
	}
	else if(new_err < -4096) //-180.F
	{
		new_err += 8192; //360.F;
	}
	// 误差积分
	err->err_integral += pid->Ki * new_err;
	//积分限幅
	if (err->err_integral > pid->limit_up)
		err->err_integral = pid->limit_up;
	else if (err->err_integral < pid->limit_dn) 
		err->err_integral = pid->limit_dn;
	//计算输出
	float out  = pid->Kp * new_err       //比例
		+ err->err_integral   //积分
		+ pid->Kd * (new_err - err->last_err); //微分
	// 更新上次误差
	err->last_err = new_err;
	return out;
}
static float pid_ctrl(int new_err/*新误差值*/, error_t *err/*旧误差*/, PID_t *pid/*控制参数*/)
{
	// 误差积分
	err->err_integral += pid->Ki * new_err;
	//积分限幅
	if (err->err_integral > pid->limit_up)
		err->err_integral = pid->limit_up;
	else if (err->err_integral < pid->limit_dn) 
		err->err_integral = pid->limit_dn;
	//计算输出
	float out  = pid->Kp * new_err       //比例
		+ err->err_integral   //积分
		+ pid->Kd * (new_err - err->last_err); //微分
	// 更新上次误差
	err->last_err = new_err;
	return out;
}
/*
********************************************************************************
*描    述 : 输出控制。
*返    回 : 无
********************************************************************************/
static void pwm_ctrl(int pwm, hd_cfg_t *hd_cfg, uint8_t dir, uint8_t *plast_pwm_dir)
{
	if (pwm >= 0)
	{
		if (*plast_pwm_dir != 0)
			TLE_ConfigUnipolarMode(hd_cfg->cs, hd_cfg->group, hd_cfg->pwni, dir); //0号芯片(U15)的第0组(1,2)配置成PWM1单极驱动,正向转动--前锁
		*plast_pwm_dir = 0;
	}
	else
	{
		if (*plast_pwm_dir != 1)
			TLE_ConfigUnipolarMode(hd_cfg->cs, hd_cfg->group, hd_cfg->pwni, ~dir); //0号芯片(U15)的第0组(1,2)配置成PWM1单极驱动,正向转动--前锁
		pwm = -pwm;
		*plast_pwm_dir = 1;
	}
	PAC_WrPWM(hd_cfg->pwni + hd_cfg->cs * 3, pwm); //--控制pwm
}

void check_dir_start(int pwm, int i)
{
	PAC_WrPWM(hd_link.hd_cfg[i].pwni + hd_link.hd_cfg[i].cs * 3, pwm);
	last_pwm_dir[i] = 2;
	pwm_ctrl(pwm, (hd_cfg_t*)&hd_link.hd_cfg[i], (eeprom_var.motor_dir >> i) & 1, &last_pwm_dir[i]);
}
int check_dir(int i)
{
	if (Speed[i] > 10.f) //前锁反馈angle/s
	{
		PAC_WrPWM(hd_link.hd_cfg[i].pwni + hd_link.hd_cfg[i].cs * 3, 0); //停止转动且刹车
		return 0;
	}
	if(Speed[i] < -10.f) //反向
	{
		eeprom_var.motor_dir ^= (1 << i);
		TLE_ConfigUnipolarMode(hd_link.hd_cfg[i].cs,
			hd_link.hd_cfg[i].group, 
			hd_link.hd_cfg[i].pwni,
			(eeprom_var.motor_dir >> i) & 1);
	}

	return 1;
}

void cal_pid_ctrl(uint32_t t_ms)
{
	int pos_set = eeprom_var.pos[NowMotor].mid;
	float spd_set;
	int cur_set = 0;
	int pwm_set;
	if (t_ms > pos_t)
	{
		pos_am = -pos_am;
		pos_t += pos_tc;
	}
	pos_set += pos_am;
	if (t_ms >= spd_t)
	{
		spd_am = -spd_am;
		spd_t += spd_tc;
	}
	spd_set = spd_am;
	if (t_ms >= cur_t)
	{
		cur_am = -cur_am;
		cur_t += cur_tc;
	}
	if (cur_am > 0) cur_set = cur_am;
	switch (eeprom_var.ctrl_loop[NowMotor])
	{
	case POSE_SPD_CUR_LOOP:
		spd_set = angle_pid_ctrl(pos_set - Angle[NowMotor], &errs[NowMotor].pos_err, &eeprom_var.pos_pids[NowMotor]);
		cur_set = pid_ctrl(spd_set - Speed[NowMotor], &errs[NowMotor].spd_err, &eeprom_var.spd_pids[NowMotor]);
		pwm_set = pid_ctrl(cur_set - Current[NowMotor], &errs[NowMotor].cur_err, &eeprom_var.cur_pids[NowMotor]);
		break;
	case CUR_LOOP:
		pwm_set = 0;
		break;
	case SPD_LOOP:
		pos_set = eeprom_var.pos[NowMotor].mid;
		if (pos_am < 0) pos_am = -pos_am;
		if (((pos_set + pos_am) <= Angle[NowMotor] && spd_set > 0) ||	//上越界
			((pos_set - pos_am) >= Angle[NowMotor] && spd_set < 0))	//下越界
		{
			spd_set = -spd_set;
			spd_t = t_ms + spd_tc;
		}
		pwm_set = pid_ctrl(spd_set - Speed[NowMotor], &errs[NowMotor].spd_err, &eeprom_var.spd_pids[NowMotor]);
		break;
	case POS_LOOP:
		pwm_set = angle_pid_ctrl(pos_set - Angle[NowMotor], &errs[NowMotor].pos_err, &eeprom_var.pos_pids[NowMotor]);
		break;
	case SPD_CUR_LOOP:
		break;
	case POS_SPD_LOOP:
		spd_set = angle_pid_ctrl(pos_set - Angle[NowMotor], &errs[NowMotor].pos_err, &eeprom_var.pos_pids[NowMotor]);
		pwm_set = pid_ctrl(spd_set - Speed[NowMotor], &errs[NowMotor].spd_err, &eeprom_var.spd_pids[NowMotor]);
		break;
	case POS_CUR_LOOP:
		break;
	default:
		break;
	}
	pwm_ctrl(pwm_set, (hd_cfg_t*)&hd_link.hd_cfg[NowMotor], (eeprom_var.motor_dir >> NowMotor) & 1, &last_pwm_dir[NowMotor]);
}
int PakeCtrlPosFr(uint8_t *pTxbuf);
uint8_t CheckSum(uint8_t *pByte, uint8_t len);
int PackHeartbeatFr(uint8_t *pTxbuf)
{
	c2p_heartbeat_t *pt = (c2p_heartbeat_t*)pTxbuf;
	pt->sync = C2P_SYNC;
	pt->frame_type = C2P_HEARTBEAT_fr;
	pt->motor = NowMotor;
	pt->work_mode = WorkMode;
	pt->angle = Angle[NowMotor];
	pt->speed = Speed[NowMotor];
	pt->current = Current[NowMotor];
	pt->check_sum = CheckSum(&pt->frame_type, sizeof(c2p_heartbeat_t) - 2);
	return sizeof(c2p_heartbeat_t);
}
#pragma endregion


/*
********************************************************************************
*描    述 :	mcu通过PAC9685产生PWM再通过TEL92108驱动马达, 
*经TLA2518反馈电流调速, 再经MT6701反馈回来角度位置。
*返    回 :	无
********************************************************************************/
//float set_angle = 300.f, set_angle2 = 267;
void MotorCtrlTask(void const * argument)
{
	//====局部变量定义
	int pwm, tmp, cnt;
	int work_mode0;
	
	//====初始化
	//初始化PWM芯片,配置PWM频率=max1526Hz,使能PWM输出
	PAC9685_Init(&hi2c1, I2C1_DoneSemHandle);
	
	//初始化MOS驱动芯片
	if (TLE92108_Init(&hspi2, SPI2_DoneSemHandle) >= 0x40)
	{
		osThreadSuspend(NULL);
	}
//	TLE_ConfigUnipolarMode(0, 0, 0, 0);//0号芯片(U15)的第0组(1,2)配置成PWM1单极驱动,正向转动--前锁
//	osDelay(20);
//	TLE_ConfigUnipolarMode(0, 1, 1, 0);//0号芯片(U15)的第1组(3,4)配置成PWM2单极驱动,正向转动--前起落架
//	osDelay(20);
//	TLE_ConfigUnipolarMode(1, 0, 0, 0);//1号芯片(U15)的第0组(1,2)配置成PWM1单极驱动,正向转动--左锁
//	osDelay(20);
//	TLE_ConfigUnipolarMode(1, 1, 1, 0); //1号芯片(U15)的第1组(3,4)配置成PWM2单极驱动,正向转动--左起落架
//	osDelay(20);
//	TLE_ConfigUnipolarMode(2, 0, 0, 0); //2号芯片(U15)的第0组(1,2)配置成PWM1单极驱动,正向转动--右锁
//	osDelay(20);
//	TLE_ConfigUnipolarMode(2, 1, 1, 0); //2号芯片(U15)的第1组(3,4)配置成PWM2单极驱动,正向转动--右起落架
//	osDelay(20);
	
	//基于角度位置编码器SSI模式: 10.5<15.625MHz
	MT6701_Init(&hspi3, RxCodEndSemHandle);
	for (size_t i = 0; i < 16; i++)
	{
		angle0[i] = -1; //初始化Angle0 < 0 使Angle0无效!
//		smpl_time[i] = osKernelSysTick();
	}
	
	//选中NC通道电流检测
	HAL_GPIO_WritePin(ADC_CHAN_SEL_GPIO_Port, ADC_CHAN_SEL_Pin, GPIO_PIN_RESET);
	TLA2518_Init(&hspi1, SPI1_DoneSemHandle, SPI1_AccessMutexHandle);

	work_mode0 = WorkMode;
	osDelay(100);
	uint32_t wake_time = osKernelSysTick();
	//====循环体
	for (;;)
	{
		osDelayUntil(&wake_time, SAMPLE_T); //osDelay(100);
		ScanCoder(NowMotor);
		//收起来:220-->放下去300-->180-->收起来300-->220
//		ScanCoder(hd_link.front_lock_cfg.codei); //--前锁
//		ScanCoder(hd_link.nose_gear_cfg.codei); //--前起落架
		osMessagePut(PC_TxMsgQueueHandle, (uint32_t)PackHeartbeatFr, 0);

		switch (WorkMode)
		{
		case STANDBY_MODE:
			if (work_mode0 != STANDBY_MODE)
			{
				work_mode0 = STANDBY_MODE;
				TLE_GotoStandbyMode(); //HB进入无效模式
				for (size_t i = 0; i < 9; i++)//所有PWM常闭
				{
					PAC_WrPWM(i, 0);
				}
			}
			break;
		case CALIBRATION_DIR:
			switch (work_mode0)
			{
			case STANDBY_MODE:
				//正向转动电机
				pwm = 1000;
				check_dir_start(pwm, NowMotor);
				work_mode0 = STANDBY_MODE + 1;
				cnt = 100;
				break;
			case STANDBY_MODE + 1:
				tmp = check_dir(NowMotor);
				cnt--;
				if (tmp == 0 || cnt == 0)
				{
					if (tmp != 0)//处理没有接电机项
					{
//						eeprom_var.pos[now_motor].laydown = eeprom_var.pos[now_motor].mid = eeprom_var.pos[now_motor].packup = 0xFFFF;
					}
					//应答控制位置帧
					osMessagePut(PC_TxMsgQueueHandle, (uint32_t)PakeCtrlPosFr, 0);
					WorkMode = STANDBY_MODE;
				}
				break;
			}
			break;
		case CALIBRATION_PID:
			cnt += 100;
			if (work_mode0 != CALIBRATION_PID)
			{
				cnt = 0;
				pos_t = pos_tc;
				spd_t = spd_tc;
				cur_t = cur_tc;
				errs[NowMotor].cur_err.err_integral = errs[NowMotor].spd_err.err_integral = errs[NowMotor].pos_err.err_integral = 0;
				last_pwm_dir[NowMotor] = 2; //!= 0, != 1
				work_mode0 = CALIBRATION_PID;
			}
			cal_pid_ctrl(cnt);
			break;
		case LANDING_GEAR_RETRACTED:
//			pwm = angle_pid_ctrl(eeprom_var.front_lock_pos.laydown - Angle[0], &front_lock_err, &eeprom_var.front_lock_angle_pid);
//			pwm_ctrl(pwm, &hd_link.front_lock_cfg, eeprom_var.front_lock_motor_dir, front_lock_err.last_pwm_dir);
			break;
		default:
			break;
		}
//		float err = set_angle - Angle[0];
//		//求最小夹角
//		if (err > 180.F)
//		{
//			err -= 360.F;
//		}
//		else if (err < -180.F)
//		{
//			err += 360.F;
//		}
//		int pwm = eeprom_var.front_lock_pos_pid[0] * err;
//		if (pwm >= 0)
//		{
//			TLE_ConfigUnipolarMode(0, 0, 0, 0); //0号芯片(U15)的第0组(1,2)配置成PWM1单极驱动,正向转动--前锁
//		}
//		else
//		{
//			TLE_ConfigUnipolarMode(0, 0, 0, 1); //0号芯片(U15)的第0组(1,2)配置成PWM1单极驱动,正向转动--前锁
//			pwm = -pwm;
//		}
//		if (pwm > 4095) test_pwm1 = 4095;
//		else test_pwm1 = pwm;
//		PAC_WrPWM(0, test_pwm1); //--前锁
		
		//收起来:224-->放下去267-->312-->收起来267-->224
//		if (Angle[1] > 180 && Angle[1] < 320)
//		{
//			err = set_angle2 - Angle[1];
//			//求最小夹角
//			if (err > 180.F)
//			{
//				err -= 360.F;
//			}
//			else if (err < -180.F)
//			{
//				err += 360.F;
//			}
//			int pwm = eeprom_var.front_lock_pos_pid[1] * err;
//			if (pwm >= 0)
//			{
//				TLE_ConfigUnipolarMode(0, 1, 1, 1); //0号芯片(U15)的第1组(3,4)配置成PWM2单极驱动,正向转动--前起落架
//			}
//			else
//			{
//				TLE_ConfigUnipolarMode(0, 1, 1, 0); //0号芯片(U15)的第1组(3,4)配置成PWM2单极驱动,正向转动--前起落架
//				pwm = -pwm;
//			}
//			if (pwm > 4095) test_pwm2 = 4095;
//			else test_pwm2 = pwm;
//			PAC_WrPWM(1, test_pwm2); //--前起落架
//		}
//		TLE_FeedDog(0);
//		TLE_FeedDog(1);
//		TLE_FeedDog(2);
	}
}